#ifndef __FOC_H__
#define __FOC_H__

#include "tim.h"
#include <stdbool.h>
#include "arm_math.h"//un use arm_mathlib need #define UNUSE_ARM_MATH_LIB 1

/************filter select****************/
#define FIRST_ORDER_LOW_PASS 0
#define KALMAN_FILTER 1
/************filter select****************/

/****************math lib******************/
#define UNUSE_ARM_MATH_LIB 0
/****************math lib******************/

/****************detect parament***********/
#define POLE_PAIRS 7
#define TIMER_PSC_SPEED  (72)-1
#define TIMER_ARR_SPEED  (1000)
#define CAL_CYCLE   ((TIMER_PSC_SPEED+1) / (72*1000000) * TIMER_ARR_SPEED) //需要大于spi1采样周期
#define DIV_ENCODER_SPI1 SPI_BAUDRATEPRESCALER_128   //72M/div 
#define TIMER_PSC_ADC  0
#define TIMER_ARR_ADC  (2000)
#define ROTOR_PHY_ANGLE (encoder_angle - rotor_zero_angle) // 转子物理角度
#define ROTOR_LOGIC_ANGLE (ROTOR_PHY_ANGLE * POLE_PAIRS)   // 转子逻辑角度（电角度）
/****************detect parament***********/

/****************pid parament***********/
#define POSITION_KP 1.5
#define POSITION_KI 0
#define POSITION_KD 0

#define SPEED_KP 0
#define SPEED_KI 0
#define SPEED_KD 0

#define TORQUE_Q_KP 0
#define TORQUE_Q_KI 0
#define TORQUE_Q_KD 0

#define TORQUE_D_KP 0
#define TORQUE_D_KI 0
#define TORQUE_D_KD 0
/****************pid parament***********/

#define MAX_MUL_CYCLE  10*PI
 
#define LOWPASS_ALAPH  0.1
#define ADC_REFERENCE_VOLT 1.65
  
#define SQRT3 1.73205080756887729353
#define deg2rad(X) (((X) * PI) / 180) 
#define rad60 deg2rad(60)
#define MIN(a,b) ((a)>(b) ?(b):(a))
#define MAX(a, b) ((a) > (b) ? (a) : (b))

typedef struct {
    float u;
    float v;
    float w;
}DutyStruct;
typedef enum
{
    null,
    position,
    speed,
    torque,
    speed_torque,
    position_speed_torque,
}PidModeEnum;
typedef struct {
    float motor_position; //rad
    float motor_speed; //deta_theta / deta_t 
    float i_u;
    float i_v;
    float i_w;
    PidModeEnum mode;
}MotorParaStruct;
extern float rotor_zero_angle;
extern float rotor_logic_angle;
extern MotorParaStruct motor_para;
extern float encoder_angle;
extern float diff_angle;
extern float mul_cycle_angle;


uint8_t CalculateCrc(uint32_t data);
void set_pwm_duty(float d_u, float d_v, float d_w);
void MotorForword(float d,float q,float phi);
DutyStruct Svpwm(int theta,float s);
static DutyStruct DescarSvpwm(float phi,float d,float q);
static void svpwm(float phi, float d, float q, float *d_u, float *d_v, float *d_w);
void foc_forward(float d, float q, float rotor_rad);
float HalfCycle(float angel,float cycle);
float LowpassFilter(float in,float last_out,float alaph);
void PidParaInit(void);
float PositionLoop(float rad);
void PidPositionControl(float rad);
void PidSpeedControl(float speed);
void PidTorqueControl(float d,float q);
void PidSpeedTorqueControl(float speed,float d,float q);
void PidPositionSpeedCtorqueControl(float rad,float speed,float d,float q);
#endif
